/******************************************************************************\
 *                                                                            *
 *                         Craftmesh Controller                               *
 *                   Flavio Bertini fl.bertini@gmail.com                      *
 *                                                                            *
 *                                                                            *
\******************************************************************************/


#include "Craftmesh_controller.h"


using namespace Leap;
using namespace std;


// Create the Leap listener and controller
Con_CraftmeshListener      _listener;
Controller                 _controller;
vector<Con_HandController> _hands;

// External resources
extern Vew_GlutWindow         _window;
extern Vew_GlutCamera         _camera;
extern vector<Mod_SubdivMesh> _mesh;
extern GLfloat _topLeftSelecPlane[3],
               _topRightSelecPlane[3],
               _bottomRightSelecPlane[3],
               _bottomLeftSelecPlane[3];


// local resources
int _handColor = 0;
vector<bool> _meshToUpdate; // check if a mesh must to be updated

// plane XZ updated by the leap
Vector _axeXupdate(1.0, 0.0, 0.0);
Vector _axeYupdate(0.0, 1.0, 0.0);
Vector _axeZupdate(0.0, 0.0, 1.0);
float  _angleXZ       = 0.0;
bool   _updateAngleXZ = false;


// move value from source to detination
template<typename T1, typename T2> void assign3values(T1 dst[3], T2 src[3])
{
  dst[0] = src[0];
  dst[1] = src[1];
  dst[2] = src[2];
  return;
}

// the camera position defines the coordinate system (rotation xz plane)
void updateAxes(void)
{
  Vector cameraPos(_camera.lookTo[0]-_camera.lookAt[0],
                   _camera.lookTo[1]-_camera.lookAt[1],
                   _camera.lookTo[2]-_camera.lookAt[2]);
  _angleXZ = cameraPos.yaw();
  _axeXupdate.x =  cos(_angleXZ);  _axeXupdate.z = sin(_angleXZ);
  _axeZupdate.x = -sin(_angleXZ);  _axeZupdate.z = cos(_angleXZ);
  return;
}


// Class Listener
Con_CraftmeshListener::Con_CraftmeshListener() {}

Con_CraftmeshListener::~Con_CraftmeshListener() {}

// function called when an instance of a Listener subclass is added to the controller
void Con_CraftmeshListener::onInit(const Controller& controller)
{
  cout << "Leap initialized: listener is ready for use" << endl;
  _meshToUpdate.resize(_mesh.size(), false);
  return;
}

// function called when a connection is established between the controller and the Leap Motion software
void Con_CraftmeshListener::onConnect(const Controller& controller)
{
  cout << "Leap connected: gestures enabled" << endl;
  controller.enableGesture(Gesture::TYPE_CIRCLE);
  controller.enableGesture(Gesture::TYPE_KEY_TAP);
  controller.enableGesture(Gesture::TYPE_SCREEN_TAP);
  controller.enableGesture(Gesture::TYPE_SWIPE);
  return;
}

// function called if the connection is losed for any reason
void Con_CraftmeshListener::onDisconnect(const Controller& controller)
{
  cout << "Disconnected" << endl;
  return;
}

// function called if the listener is removed from the controller or the controller is destroyed
void Con_CraftmeshListener::onExit(const Controller& controller)
{
  cout << "Exited: bye bye!" << endl;
  return;
}

// recover and manage the most recent frame form the Leap
void Con_CraftmeshListener::onFrame(const Controller& controller)
{
  // define which mesh are under modifying
  vector<bool> meshWorkingProgress;
  meshWorkingProgress.resize(_mesh.size(), false);

  // get the most recent frame
  const Frame frame = controller.frame();

  // check the available hands
  if (!frame.hands().isEmpty())
  {
    // CAMERA MODE
    if (_window.mode == camera)
    {
      // camera is controlled by a fingers
      bool updatePos = false,  updateZoom = false;
      float oldZoom = _camera.zooming;
      // the index of the hand with only one straight finger
      int indexLookHand = NULLREF;
      // the index of the hand with five visible fingers
      int indexZoomHand = NULLREF;
      for (int i=0; i<frame.hands().count(); i++)
      {
        int numFingers = frame.hands()[i].fingers().extended().count();
        if (numFingers == LEAP_OFOPEN) indexLookHand = i;
        if (numFingers == LEAP_HFOPEN) indexZoomHand = i;
      }

      // extract hand and finger information to manage the camera movements
      // object one-finger-hand: update camera position and direction
      if (indexLookHand != NULLREF)
      {
        updatePos = true;
        Mat_Vector tipPos = static_cast<Mat_Vector> (frame.hands()[indexLookHand].fingers().extended()[0].stabilizedTipPosition());
        Mat_Vector tipDir = static_cast<Mat_Vector> (frame.hands()[indexLookHand].fingers().extended()[0].direction());

        // rotation imposed by the camera
        mat_glRotateXZ(_angleXZ, &tipPos);
        mat_glRotateXZ(_angleXZ, &tipDir);

        // define lookAt vector
        // polar coordinates of the direction vector
        tipDir.mat_cartesian2sphere();
        tipDir.rho = GL_LOOK_RHO/2.0;
        // position of the lookTo and lookAt vector starting from new polar coordinates
        tipDir.mat_sphere2cartesian();
        // the tipPos will be in the middle
        _camera.lookAt[0] = -(tipDir.x) + tipPos.x;
        _camera.lookAt[1] = -(tipDir.y) + tipPos.y + LEAP_XZ_PLANE;
        _camera.lookAt[2] = -(tipDir.z) + tipPos.z;
        _camera.lookTo[0] = tipDir.x + tipPos.x;
        _camera.lookTo[1] = tipDir.y + tipPos.y + LEAP_XZ_PLANE;
        _camera.lookTo[2] = tipDir.z + tipPos.z;
        // angleXZ to be updated on exit from camera mode
        _updateAngleXZ = true;
      }
      // object five-fingers-hand: update zoom information
      else if (indexZoomHand != NULLREF)
      {
        // hand translation between this five-fingers-hand and the same one in the previous frame
        Vector zoomVector = frame.hands()[indexZoomHand].translation(controller.frame(LEAP_ZOOM_GAP));
        if (zoomVector.magnitude() > 2.0)
        {
          updateZoom = true;
          // zoom in and zoom out
          if (zoomVector.y < 0) _camera.vew_zoomIn(1.0);
          else _camera.vew_zoomOut(1.0);
        }
      }

      // update camera
      if (updatePos || updateZoom)
      {
        _camera.vew_applyZoom(updateZoom, oldZoom);
        _camera.vew_createEyes();
      }
    }
    else if (_window.mode == object)  // OBJECT MODE
    {
      // update angleXZ
      if (_updateAngleXZ)
      {
        // update coordinate system axes
        updateAxes();
        _updateAngleXZ = false;
      }

      // check visible hands
      vector<int> handCheck(_hands.size(), 0);
      for (int i=0; i<frame.hands().count(); i++)
      {
        bool alreadyPresent = false;
        for (unsigned int j=0; j<_hands.size(); j++)
        {
          if (_hands[j].handRef.id() == frame.hands()[i].id())  // this hand was already present: update
          {
            _hands[j].handRef = frame.hands()[i];
            _hands[j].fingRef = frame.hands()[i].fingers().extended();
            alreadyPresent = true;
            handCheck[j] = 1;
          }
        }
        if (!alreadyPresent)  // this hand wasn't already present: insert
        {
          handCheck.resize(_hands.size()+1);
          handCheck[handCheck.size()-1] = 1;
          _hands.resize(_hands.size()+1);
          _hands[_hands.size()-1].handRef   = frame.hands()[i];
          _hands[_hands.size()-1].fingRef   = frame.hands()[i].fingers().extended();
          _hands[_hands.size()-1].meshRef   = NULLREF;
          _hands[_hands.size()-1].frameTap  = 0;
          _hands[_hands.size()-1].firstFist = true;
          _hands[_hands.size()-1].selecRadius = SSRADIUS;
          if (frame.hands()[i].palmPosition().x > 0) _hands[_hands.size()-1].left_right = RIGHTH;
          else _hands[_hands.size()-1].left_right = LEFTH;
          assign3values(_hands[_hands.size()-1].color, GL_COLOR_H[_handColor%GL_NUM_COLORH]);
          _handColor++;
        }
      }
      // this hand is no longer present: remove
      for (int i=handCheck.size()-1; i>=0; i--)
      {
        if (!handCheck[i])
        {
          if (_hands[i].meshRef >= 0)
          {
            _mesh[_hands[i].meshRef].handId   = NULLREF;
            _mesh[_hands[i].meshRef].captured = false;
          }
          _hands.erase(_hands.begin()+i);
        }
      }

      // from here on out only verified hands
      // define translation and rotation
      for (unsigned int j=0; j<_hands.size(); j++)
      {

        // all fingers are closed --> translation
        if (_hands[j].fingRef.count() == LEAP_FFCLOSE && _hands[j].meshRef >= 0)
        {
          Vector transl;
          if (_hands[j].firstFist)  // hooking the mesh: mesh center == fist center
          {
            _hands[j].firstFist = false;
            Mat_Vector palmPos = _hands[j].handRef.stabilizedPalmPosition();
            palmPos.y += LEAP_XZ_PLANE;
            mat_glRotateXZ(_angleXZ, &palmPos);      // rotation imposed by the camera
            transl = palmPos - _mesh[_hands[j].meshRef].center;  // hooking
            _hands[j].frameTrans = frame;
          }
          else
          {
            if (_hands[j].handRef.translationProbability(_hands[j].frameTrans) >= 0.0)
            {
              transl = _hands[j].handRef.translation(_hands[j].frameTrans);
              _hands[j].frameTrans = frame;
              mat_glRotateXZ(_angleXZ, &transl);     // rotation imposed by the camera
            }
          }
          _mesh[_hands[j].meshRef].transl += transl;
          _mesh[_hands[j].meshRef].mod_translateCenter(transl);
          _meshToUpdate[_hands[j].meshRef] = true;
          meshWorkingProgress[_hands[j].meshRef] = true;
        }
        else
        {
          _hands[j].firstFist = true;
        }

        // four fingers are opened --> rotation
        if (_hands[j].fingRef.count() == LEAP_FFOPEN && _hands[j].meshRef >= 0)
        {
          Mat_Vector palmNormal = static_cast<Mat_Vector> (_hands[j].handRef.palmNormal());
          Mat_Vector palmDirect = static_cast<Mat_Vector> (_hands[j].handRef.direction());
          float axRot[3];
          int axIndex;
          axRot[0] = palmNormal.pitch()+(PI/2.0);  // the palm normal pitch angle define X rotation
          axRot[1] = -1.0*palmDirect.yaw();        // the palm direction yaw angle define Y rotation
          axRot[2] = palmNormal.roll();            // the palm normal roll angle define Z rotation
          float maxRot = 0.0;
          for (int i=0; i<3; i++)
          {
            if (fabs(axRot[i]) > maxRot)
            {
              maxRot  = fabs(axRot[i]);
              axIndex = i;
            }
          }

          // define rotation
          if (fabs(axRot[axIndex]) > 0.2)
          {
            float angle = -(axRot[axIndex] - (axRot[axIndex] > 0.0 ? 0.2 : -0.2) )/80.0;
            float rotMatrix[16];
            switch (axIndex)
            {
              case X:
                mat_computeRotationMatrixFix(_axeXupdate, angle, rotMatrix);
                break;
              case Y:
                mat_computeRotationMatrixFix(_axeYupdate, angle, rotMatrix);
                break;
              case Z:
                mat_computeRotationMatrixFix(_axeZupdate, angle, rotMatrix);
                break;
            }
            mat_glMatrixfMultiply(_mesh[_hands[j].meshRef].rotat, rotMatrix, _mesh[_hands[j].meshRef].rotat);
            mat_glMatrixInvert(_mesh[_hands[j].meshRef].rotat, _mesh[_hands[j].meshRef].invrot);
            _meshToUpdate[_hands[j].meshRef] = true;
            meshWorkingProgress[_hands[j].meshRef] = true;
          }
        }

      }
    }
    else if (_window.mode == edit)  // EDIT MODE
    {
      // update angleXZ
      if (_updateAngleXZ)
      {
        // update coordinate system axes
        updateAxes();
        _updateAngleXZ = false;
      }

      // check visible hands
      vector<int> handCheck(_hands.size(), 0);
      for (int i=0; i<frame.hands().count(); i++)
      {
        bool alreadyPresent = false;
        for (unsigned int j=0; j<_hands.size(); j++)
        {
          if (_hands[j].handRef.id() == frame.hands()[i].id())  // this hand was already present: update
          {
            _hands[j].handRef = frame.hands()[i];
            _hands[j].fingRef = frame.hands()[i].fingers().extended();
            alreadyPresent = true;
            handCheck[j] = 1;
          }
        }
        if (!alreadyPresent)  // this hand wasn't already present: insert
        {
          handCheck.resize(_hands.size()+1);
          handCheck[handCheck.size()-1] = 1;
          _hands.resize(_hands.size()+1);
          _hands[_hands.size()-1].handRef   = frame.hands()[i];
          _hands[_hands.size()-1].fingRef   = frame.hands()[i].fingers().extended();
          _hands[_hands.size()-1].meshRef   = NULLREF;
          _hands[_hands.size()-1].frameTap  = 0;
          _hands[_hands.size()-1].firstFist = true;
          _hands[_hands.size()-1].selecRadius = SSRADIUS;
          assign3values(_hands[_hands.size()-1].color, GL_COLOR_H[_handColor%GL_NUM_COLORH]);
          _handColor++;
        }
      }
      // this hand is no longer present: remove
      for (int i=handCheck.size()-1; i>=0; i--)
      {
        if (!handCheck[i])
        {
          if (_hands[i].meshRef >= 0)
          {
            _mesh[_hands[i].meshRef].handId   = NULLREF;
            _mesh[_hands[i].meshRef].captured = false;
          }
          _hands.erase(_hands.begin()+i);
        }
      }
    }
    else if (_window.mode == cut)  // CUT MODE
    {
      cout << "cut mode - working in progress ..." << endl;
    }
    else if (_window.mode == boolean)  // BOOLEAN MODE
    {
      // update angleXZ
      if (_updateAngleXZ)
      {
        // update coordinate system axes
        updateAxes();
        _updateAngleXZ = false;
      }

      // check visible hands
      vector<int> handCheck(_hands.size(), 0);
      for (int i=0; i<frame.hands().count(); i++)
      {
        bool alreadyPresent = false;
        for (unsigned int j=0; j<_hands.size(); j++)
        {
          if (_hands[j].handRef.id() == frame.hands()[i].id())  // this hand was already present: update
          {
            _hands[j].handRef = frame.hands()[i];
            _hands[j].fingRef = frame.hands()[i].fingers().extended();
            alreadyPresent = true;
            handCheck[j] = 1;
          }
        }
        if (!alreadyPresent)  // this hand wasn't already present: insert
        {
          handCheck.resize(_hands.size()+1);
          handCheck[handCheck.size()-1] = 1;
          _hands.resize(_hands.size()+1);
          _hands[_hands.size()-1].handRef   = frame.hands()[i];
          _hands[_hands.size()-1].fingRef   = frame.hands()[i].fingers().extended();
          _hands[_hands.size()-1].meshRef   = NULLREF;
          _hands[_hands.size()-1].frameTap  = 0;
          _hands[_hands.size()-1].firstFist = true;
          _hands[_hands.size()-1].selecRadius = SSRADIUS;
          assign3values(_hands[_hands.size()-1].color, GL_COLOR_H[_handColor%GL_NUM_COLORH]);
          _handColor++;
        }
      }
      // this hand is no longer present: remove
      for (int i=handCheck.size()-1; i>=0; i--)
      {
        if (!handCheck[i])
        {
          if (_hands[i].meshRef >= 0)
          {
            _mesh[_hands[i].meshRef].handId   = NULLREF;
            _mesh[_hands[i].meshRef].captured = false;
          }
          _hands.erase(_hands.begin()+i);
        }
      }
    }
    else if (_window.mode == sculpture)  // SCULPTURE MODE
    {
      cout << "sculpture mode - working in progress ..." << endl;
    }

  }
  else
  {
    // no visible hands: set free hand
    if (_hands.size() != 0)
    {
      for (unsigned int j=0; j<_hands.size(); j++)
      {
        if (_hands[j].meshRef >= 0)
        {
          _mesh[_hands[j].meshRef].handId   = NULLREF;
          _mesh[_hands[j].meshRef].captured = false;
        }
      }
      _hands.resize(0);
    }
    // no visible hands: set free mesh
    for (unsigned int j=0; j<_mesh.size(); j++)
    {
      _mesh[j].captured = false;
    }
  }

  // explore the available gestures gestures' list
  const GestureList gesturesList = frame.gestures();
  for (int g = 0; g < gesturesList.count(); ++g)
  {
    Gesture gesture = gesturesList[g];
    switch (gesture.type()) {
      case Gesture::TYPE_CIRCLE:
      {
        // to scale the grabbed mesh
        if (_window.mode == object)
        {
          CircleGesture circle = gesture;
          // check if the hand has already a hand: put down it
          for (unsigned int j=0; j<_hands.size(); j++)
          {
            if (_hands[j].handRef.id() == circle.pointable().hand().id() && _hands[j].meshRef >= 0)
            {
              float scaleFactor = 1.0;
              // the second fingers could define the scale axis
              // in object coordinates until application
              if (circle.pointable().hand().fingers().extended().count() == LEAP_TFOPEN)
              {
                // check if the circle is drawn in clockwise or counterclockwise
                if (circle.pointable().direction().angleTo(circle.normal()) <= PI/4)
                  scaleFactor = circle.radius()*LEAP_SCALE+1;
                else
                  scaleFactor = 1.0/(circle.radius()*LEAP_SCALE+1);

                // define the axis
                float scale[16];
                mat_glMatrixfIdentity(scale);
                float rotat[16];            // scale operation orthogonal to the view vector
                float rotatInv[16];
                mat_glMatrixfIdentity(rotat);
                mat_glMatrixfIdentity(rotatInv);
                Mat_Vector normal = static_cast<Mat_Vector> (circle.normal());
                normal.mat_cartesian2sphere();
                if (fabs(normal.theta) < (65.0*DEG_TO_RAD) || fabs(normal.theta) > (110.0*DEG_TO_RAD))
                {
                  _mesh[_hands[j].meshRef].scale[5]  *= scaleFactor;
                  _mesh[_hands[j].meshRef].invsca[5] *= (1.0/scaleFactor);
                }
                else if (fabs(normal.x) > fabs(normal.z))
                {
//                  _mesh[_hands[j].meshRef].scale[0]  *= scaleFactor;
//                  _mesh[_hands[j].meshRef].invsca[0] *= (1.0/scaleFactor);
                  scale[0] *= scaleFactor;
                  mat_glMatrixYrot(_angleXZ, rotat);
                  mat_glMatrixYrot(-_angleXZ, rotatInv);
                  mat_glMatrixfMultiply(scale, rotat, scale);
                  mat_glMatrixfMultiply(rotatInv, scale, scale);
                  mat_glMatrixfMultiply(scale, _mesh[_hands[j].meshRef].scale, _mesh[_hands[j].meshRef].scale);
                  mat_glMatrixInvert(_mesh[_hands[j].meshRef].scale, _mesh[_hands[j].meshRef].invsca);
                }
                else
                {
//                  _mesh[_hands[j].meshRef].scale[10]  *= scaleFactor;
//                  _mesh[_hands[j].meshRef].invsca[10] *= (1.0/scaleFactor);
                  scale[10] *= scaleFactor;
                  mat_glMatrixYrot(_angleXZ, rotat);
                  mat_glMatrixYrot(-_angleXZ, rotatInv);
                  mat_glMatrixfMultiply(scale, rotat, scale);
                  mat_glMatrixfMultiply(rotatInv, scale, scale);
                  mat_glMatrixfMultiply(scale, _mesh[_hands[j].meshRef].scale, _mesh[_hands[j].meshRef].scale);
                  mat_glMatrixInvert(_mesh[_hands[j].meshRef].scale, _mesh[_hands[j].meshRef].invsca);
                }
                _mesh[_hands[j].meshRef].rCapture *= scaleFactor;
                _meshToUpdate[_hands[j].meshRef]   = true;
                meshWorkingProgress[_hands[j].meshRef] = true;
              }
              else  // with only one finger the whole object is scaled
              {
                // check if the circle is drawn in clockwise or counterclockwise
                if (circle.pointable().direction().angleTo(circle.normal()) <= PI/4)
                  scaleFactor = circle.radius()*LEAP_SCALE+1;
                else
                  scaleFactor = 1.0/(circle.radius()*LEAP_SCALE+1);
                _mesh[_hands[j].meshRef].scale[0]   *= scaleFactor;
                _mesh[_hands[j].meshRef].invsca[0]  *= (1.0/scaleFactor);
                _mesh[_hands[j].meshRef].scale[5]   *= scaleFactor;
                _mesh[_hands[j].meshRef].invsca[5]  *= (1.0/scaleFactor);
                _mesh[_hands[j].meshRef].scale[10]  *= scaleFactor;
                _mesh[_hands[j].meshRef].invsca[10] *= (1.0/scaleFactor);
                _mesh[_hands[j].meshRef].rCapture   *= scaleFactor;
                _meshToUpdate[_hands[j].meshRef] = true;
                meshWorkingProgress[_hands[j].meshRef] = true;
              }

              // verifies that the capture radius lies within the limits
              if (_mesh[_hands[j].meshRef].rCapture < MIN_RCAPTURE)
                _mesh[_hands[j].meshRef].rCapture = MIN_RCAPTURE;
              else if (_mesh[_hands[j].meshRef].rCapture > MAX_RCAPTURE)
                _mesh[_hands[j].meshRef].rCapture = MAX_RCAPTURE;

            }
          }
        }
        else if (_window.mode == edit)
        {
          CircleGesture circle = gesture;
          // a hand with one finger on to change the size of the sphere
          if (_window.selecSphere)
          {
            for (unsigned int j=0; j<_hands.size(); j++)
            {
              if (_hands[j].handRef.id() == circle.pointable().hand().id() &&
                  circle.pointable().hand().fingers().extended().count() == LEAP_OFOPEN)
              {
                float scaleFactor = 1.0;

                // check if the circle is drawn in clockwise or counterclockwise
                if (circle.pointable().direction().angleTo(circle.normal()) <= PI/4)
                  scaleFactor = circle.radius()*(LEAP_SCALE*1.5)+1;
                else
                  scaleFactor = 1.0/(circle.radius()*(LEAP_SCALE*1.5)+1);

                _hands[j].selecRadius *= scaleFactor;
              }
            }
          }
          // a hand with one finger on to change the size of the plane
          else if (_window.selecPlane)
          {
            for (unsigned int j=0; j<_hands.size(); j++)
            {
              if (_hands[j].handRef.id() == circle.pointable().hand().id() &&
                  circle.pointable().hand().fingers().extended().count() == LEAP_OFOPEN)
              {
                float scaleFactor = 0.0;

                // check if the circle is drawn in clockwise or counterclockwise
                if (circle.pointable().direction().angleTo(circle.normal()) <= PI/4)
                  scaleFactor =  0.5f;
                else
                  scaleFactor = -0.5f;

                _topLeftSelecPlane[0] -= scaleFactor;      _topLeftSelecPlane[2] -= scaleFactor;
                _topRightSelecPlane[0] += scaleFactor;     _topRightSelecPlane[2] -= scaleFactor;
                _bottomRightSelecPlane[0] += scaleFactor;  _bottomRightSelecPlane[2] += scaleFactor;
                _bottomLeftSelecPlane[0] -= scaleFactor;   _bottomLeftSelecPlane[2] += scaleFactor;
              }
            }
          }
        }
        break;
      }
      case Gesture::TYPE_SWIPE:
      {
        break;
      }
      case Gesture::TYPE_KEY_TAP:
      {
        // This code is similar for selection by KEY 'c'
        // to select/deselect a mesh
        if (_window.mode == object || _window.mode == edit)
        {
          // from the gesture to the hand's id through the finger
          KeyTapGesture tap = gesture;
          Vector tapPos     = tap.position();
          tapPos.y += LEAP_XZ_PLANE;
          mat_glRotateXZ(_angleXZ, &tapPos);
          int tapHandId     = tap.pointable().hand().id();
          bool putDown      = false;

          // check to avoid repeated tap gesture
          bool repeatedTap = false;
          for (unsigned int j=0; j<_hands.size(); j++)
          {
            if (_hands[j].handRef.id() == tapHandId && _hands[j].frameTap == frame.id())
            {
              repeatedTap = true;
            }
          }

          if (!repeatedTap)
          {
            // check if the hand has already a mesh: put down it
            for (unsigned int j=0; j<_hands.size(); j++)
            {
              if (_hands[j].handRef.id() == tapHandId && _hands[j].meshRef >= 0)
              {
                _mesh[_hands[j].meshRef].captured = false;
                _mesh[_hands[j].meshRef].handId   = NULLREF;
                _hands[j].meshRef  = NULLREF;
                _hands[j].frameTap = frame.id();
                putDown = true;
              }
            }

            // check the nearest free mesh to pick up
            if (!putDown)
            {
              int pickupMeshRef = NULLREF;
              float tmpMinDistance = numeric_limits<float>::max();
              for (unsigned int j=0; j<_mesh.size(); j++)
              {
                // inside the sphere of the mesh && the mesh is free && the distance is the nearest
                if ( _mesh[j].center.distanceTo(tapPos) <= _mesh[j].rCapture &&
                    !_mesh[j].captured && _mesh[j].center.distanceTo(tapPos) < tmpMinDistance)
                {
                  pickupMeshRef = j;
                  tmpMinDistance = _mesh[j].center.distanceTo(tapPos);
                }
              }
              if (pickupMeshRef >= 0)
              {
                _mesh[pickupMeshRef].captured = true;
                _mesh[pickupMeshRef].handId   = tapHandId;
                for (unsigned int j=0; j<_hands.size(); j++)
                {
                  if (_hands[j].handRef.id() == tapHandId)
                  {
                    _hands[j].meshRef  = pickupMeshRef;
                    _hands[j].frameTap = frame.id();
                  }
                }
              }
            }
          }
        }
        break;
      }
      case Gesture::TYPE_SCREEN_TAP:
      {
        break;
      }
      default:
      {
        cout << "Unknown gesture type." << endl;
        break;
      }
    }
  }

  // apply the transformations only when they are completely finished
  for (unsigned int i=0; i<meshWorkingProgress.size(); i++)
  {
    if (!meshWorkingProgress[i] && _meshToUpdate[i])
    {
      _meshToUpdate[i] = false;
      _mesh[i].mod_applyTransformations();
    }
  }

  return;
}

void Con_CraftmeshListener::onFocusGained(const Controller& controller)
{
  cout << "Focus Gained" << endl;
  return;
}

void Con_CraftmeshListener::onFocusLost(const Controller& controller)
{
  cout << "Focus Lost" << endl;
  return;
}
